#ifndef UTILS_H
#define UTILS_H

/**
 * @brief 计算像素点(x, y)实际离机器人的距离(三维抛物面)
 */
inline float pixToDistance(int x, int y, int ROBOT_PIX_X, int ROBOT_PIX_Y){
	static int tmp[5] = {1};
	static const float coe[]={     -1.2465199e+01f,   6.1950704e-03f,   9.4667087e-03f,   1.2634282e-01f,   1.0160014e-02f};
	float r = 0.0f;
	x -= ROBOT_PIX_X; y -= ROBOT_PIX_Y;
	tmp[1] = x;		tmp[2] = x*x;		tmp[3] = y;		tmp[4] = y*y;
	for(int i=0;i<5;i++) r += tmp[i]*coe[i];
	return abs(r); 
}

#endif // end UTILS_H
